#!/usr/bin/env python


from copy import deepcopy
import rospy
from visualization_msgs.msg import Marker
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Quaternion
import tf
import math
import numpy as np
from geometry_msgs.msg import Point 
 
def pubObstacle(ob_pub, type,mid, x,y,z,sx,sy,sz,tx="1"):
    shape = []
    if type==0:
        shape = Marker.CUBE
    elif type==1:
        shape = Marker.SPHERE
    elif type==2:
        shape = Marker.ARROW
    elif type==3:
        shape = Marker.CYLINDER
    else:
        shape = Marker.TEXT_VIEW_FACING
    marker = Marker()  # 定义Marker类型的变量
    marker.header.frame_id = "world"  # 设置header的frame——id和stamp属性值
    marker.header.stamp = rospy.Time.now()  # 这里应该是一个浮点值，对应cpp版本 ros::Time::now()
    # 下面两个量定义了该marker的命名空间和id， 通过这两个量能够确定marker的身份，如果身份相同，那么则刷新rviz中对应目标。后面我们会讲到marker是什么
    marker.ns = "basic_shapes"
    marker.id = mid
    
    marker.type = shape  # marker的类型
    marker.action = Marker.ADD # marker action

    # 初始化了marker的位置和初始的角度
    marker.pose.position.x = x
    marker.pose.position.y = y
    marker.pose.position.z = z
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0
    # marker的尺寸大小，这里1.0对应于现实地图的1m
    marker.scale.x = sx
    marker.scale.y = sy
    marker.scale.z = sz
    # marker的颜色定义， 注意透明度a的设置，为0就看不到了
    marker.color.r = 155/255.0
    marker.color.g = 155/255.0
    marker.color.b = 155/255.0
    marker.color.a = 1.0

    marker.lifetime = rospy.Duration()
    ob_pub.publish(marker)  # 发布消息
    
    # 每次发布的消息不同，从而让rviz绘制不同的marker

def DataUpdating(path_pub, path_record, darw_path):

    # 时间戳
    current_time = rospy.Time.now()

    # 四元素转换
    # quat = tf.transformations.quaternion_from_euler(0, 0, th)
    path_record.header.frame_id = "world"
    path_record.header.stamp = current_time

    for i in range(len(darw_path)):    
        # 配置姿态
        pose = PoseStamped()
        pose.header.stamp = current_time
        pose.header.frame_id = 'world'
        pose.pose.position.x = darw_path[i,0]
        pose.pose.position.y = darw_path[i,1]
        pose.pose.position.z = darw_path[i,2]
        pose.pose.orientation.x = 0
        pose.pose.orientation.y = 0
        pose.pose.orientation.z = 0
        pose.pose.orientation.w = 0

        # 配置路径
        path_record.poses.append(pose)
def DataUpdating2( path_record, darw_path):

    # 时间戳
    current_time = rospy.Time.now()

    # 四元素转换
    # quat = tf.transformations.quaternion_from_euler(0, 0, th)
    path_record.header.frame_id = "world"
    path_record.header.stamp = current_time


    pose = PoseStamped()
    pose.header.stamp = current_time
    pose.header.frame_id = 'world'
    pose.pose.position.x = darw_path[0]
    pose.pose.position.y = darw_path[1]
    pose.pose.position.z = darw_path[2]
    pose.pose.orientation.x = 0
    pose.pose.orientation.y = 0
    pose.pose.orientation.z = 0
    pose.pose.orientation.w = 0

    # 配置路径
    path_record.poses.append(pose)
    # 发布路径
    
# def DataUpdating(path_pub, marker, darw_path):
#     marker.header.frame_id = "world"  # 设置header的frame——id和stamp属性值
#     marker.header.stamp = rospy.Time.now()  # 这里应该是一个浮点值，对应cpp版本 ros::Time::now()
#     # 下面两个量定义了该marker的命名空间和id， 通过这两个量能够确定marker的身份，如果身份相同，那么则刷新rviz中对应目标。后面我们会讲到marker是什么
#     marker.ns = "basic_shapes"
#     marker.id = 99
    
#     marker.type = Marker.LINE_STRIP  # marker的类型
#     # marker.action = Marker.ADD # marker action

#     # 初始化了marker的位置和初始的角度
#     for i in range(len(darw_path)):    
#         point = Point()
#         point.x = darw_path[i,0]
#         point.y = darw_path[i,1]
#         point.z = darw_path[i,2]
#         marker.points.append(deepcopy(point))
#     marker.scale.x = 0.01
#     marker.scale.y = 0.01
#     marker.scale.z = 0.01
#     # marker的颜色定义， 注意透明度a的设置，为0就看不到了
#     marker.color.r = 155/255.0
#     marker.color.g = 155/255.0
#     marker.color.b = 155/255.0
#     marker.color.a = 1.0

#     return marker
    # 发布路径      
def node(darw_path, darw_path2,  darw_path3):
    """
    节点启动函数
    """
    try:

        # 初始化节点path
        rospy.init_node('PathRecord')

        # 定义发布器 path_pub 发布 trajectory
        path_pub = rospy.Publisher('trajectory', Path, queue_size=50)
        path_pub2 = rospy.Publisher('trajectory2', Path, queue_size=50)
        path_pub3 = rospy.Publisher('trajectory3', Path, queue_size=50)

        # 初始化循环频率
        rate = rospy.Rate(30)

        # 定义路径记录
        path_record = Path()
        # path_record = Marker()  # 定义Marker类型的变量

        path_record2 = Path()
        path_record3 = Path()

        DataUpdating(path_pub, path_record, darw_path)
        DataUpdating(path_pub2, path_record2, darw_path2)
        DataUpdating(path_pub3, path_record3, darw_path3)

        ob_pub = rospy.Publisher("visualization_marker", Marker, queue_size=1)
        ob_pub2 = rospy.Publisher("visualization_marker1", Marker, queue_size=1)
        ob_pub3 = rospy.Publisher("visualization_marker2", Marker, queue_size=1)
        start_pub = rospy.Publisher("visualization_marker3", Marker, queue_size=1)
        goal_pub = rospy.Publisher("visualization_marker4", Marker, queue_size=1)
        
        # 在程序没退出的情况下
        ob = np.array([
        [0.42,0,0.28],
        [0.35,-0.1,0.23]])
        ob[0,0] = ob[0,0] - 0.001*0
        count = 0
        while not rospy.is_shutdown():
            # ob[0,0] = ob[0,0] - 0.001

            # 数据更新函数
            # 时间戳
            current_time = rospy.Time.now()

            # 发布tf
            # br = tf.TransformBroadcaster()
            # br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
            #          rospy.Time.now(), "odom", "world")
            pubObstacle(ob_pub,1,0,  ob[0,0],0,0.28,0.1,0.1,0.1)
            pubObstacle(ob_pub2,1,0,  0.35,-0.1,0.23,0.1,0.1,0.1)
            # pubObstacle(ob_pub3,3,2, 6,6,5,1,1,3)
            pubObstacle(start_pub,0,3, darw_path2[0,0],darw_path2[0,1],darw_path2[0,2],0.01,0.01,0.01)
            pubObstacle(goal_pub,1,4, darw_path2[-1,0],darw_path2[-1,1],darw_path2[-1,2],0.01,0.01,0.01)
            
            # if count<darw_path.shape[0]:
            #     DataUpdating2(path_record, darw_path[count])
            # # print(path_record.poses[-1])
            path_pub.publish(path_record)   
            path_pub2.publish(path_record2)   
            path_pub3.publish(path_record3)   
            
            # # ob[0,0] = ob[0,0] - 0.01
            # # 休眠
            # count = count + 1 
            rate.sleep()

    except rospy.ROSInterruptException:
        pass

# def node2(darw_path):
    
if __name__ == '__main__':
    node()
